

from operator import index
import numpy as np

from compute_f import *

# def curve_line_segmentation(ori, pos_delta, slide_w):
#     # 先找长度为slide_w下ori大于45°的段
#     ang = 70
#     data_length = len(ori)
#     data_line_seg = []
#     data_cor_seg = []

#     index_start, index_stop = 0, 0
#     while data_length > index_stop:
#         ang_delta = abs(ori[index_stop] - ori[index_start])
#         if ang_delta > 360 - ang:
#             ang_delta = 360 - ang_delta
#         if ang_delta > 45: #找到了第一个弯道
#             index_start = np.searchsorted(ori[0:index_stop], ori[index_stop] - ori[index_start], side='right') - 1
#             tmp_start = index_start #第一个45°角起始点
#             index_stop += 1
#             pathsum = 0

#             while True:
#                 pathsum += pos_delta[index_stop]
#                 if pathsum > 10:
#                     pathsum = 0
#                     ang_delta = abs(ori[index_stop] - ori[tmp_start])
#                     tmp_start = index_stop
#                     if ang_delta > 360 - ang:
#                         ang_delta = 360 - ang_delta
#                     if ang_delta > 20:
#                         index_stop += 1
#                     else:
#                         index_stop = np.searchsorted(ori[tmp_start:index_stop], ori[index_stop] - ori[tmp_start], side='right') - 1 + tmp_start
#                         data_cor_seg.append([index_start, index_stop]) #添加第一个弯道
#                         index_start = index_stop
#                         break
#                 else: index_stop += 1  
#             break #找完了第一个弯道
         
#     pathsum = 0
#     while data_length > index_stop:
#         index_stop += 1 
#         ang_delta = abs(ori[index_stop] - ori[index_start])
#         if ang_delta > 360 - ang:
#             ang_delta = 360 - ang_delta
#         if ang_delta > ang:
#             tmp_stop = np.searchsorted(ori[index_start:index_stop], ori[index_stop] - ori[tmp_start], side='right') - 1

#             index_stop += 1



#         ang_delta = abs(ori[index_stop] - ori[index_start])
#         if ang_delta > 360 - ang:
#             ang_delta = 360 - ang_delta
#         if ang_delta > ang:
#             data_seg.append([index_start, index_stop])
#             index_stop += 1
#             index_start = index_stop
#         else:
#             index_stop += 1
#     data_seg.append([index_start, index_stop])
#     return np.array(data_seg)

def curve_line_segmentation(ori, pos_delta, slide_w):
    # 先找长度为slide_w下ori大于45°的段
    ang = 70
    data_length = len(ori)
    # data_seg = []
    data_cor_seg = []

    index_start, index_stop = 0, 0
    while data_length > index_stop:
        ang_delta = abs(ori[index_stop] - ori[index_start])
        if ang_delta > 360 - ang:
            ang_delta = 360 - ang_delta
        if ang_delta > ang:
            index_cor_start = np.searchsorted(ori[index_start:index_stop], ori[index_start], side='right') - 1 + index_start #找弯道起始点
            # data_cor_seg.append([index_cor_start, index_stop])
            # index_start = index_stop
            pathsum = 0
            index_tmp = index_stop-1
            while True:
                index_stop += 1
                pathsum += pos_delta[index_stop]
                if pathsum > 10:
                    pathsum = 0
                    ang_delta = abs(ori[index_stop] - ori[index_tmp])
                    if ang_delta > 360 - ang:
                        ang_delta = 360 - ang_delta
                    if ang_delta > 30:
                        index_cor_stop = np.searchsorted(ori[index_tmp:index_stop], ori[index_tmp], side='right') - 1 + index_tmp #找弯道结束点
                        data_cor_seg.append([index_cor_start, index_cor_stop]) #添加第一个弯道
                        index_start = index_cor_stop
                    else: 
                        data_cor_seg.append([index_cor_start, index_tmp])
                        index_start = index_tmp    
                    break
        else: index_stop += 1
    return np.array(data_cor_seg)


# def curve_line_segmentation(ori, pos_delta, slide_w):
# # 先找长度为slide_w下ori大于45°的段
#     ang = 70
#     data_length = len(ori)
#     data_seg = []

#     index_start, index_stop = 0, 0
#     while data_length > index_stop:
#         ang_delta = abs(ori[index_stop] - ori[index_start])
#         if ang_delta > 360 - ang:
#             ang_delta = 360 - ang_delta
#         if ang_delta > ang:
#             data_seg.append([index_start, index_stop])
#             index_stop += 1
#             index_start = index_stop
#         else:
#             index_stop += 1
#     data_seg.append([index_start, index_stop])
#     return np.array(data_seg)
def grid_traj(pos_mag):
    """将轨迹格点化，格子大小为1 m
    
    Args:
        pos_mag: #0 time, 1 X, 2 Y, 3 Z, 4 ori，5 pos_delta, 6 magx, 7 magy, 8 magz, 9 mag_xyz
        
        
    Returns:
        格点化后的格子数据: # 0 X, 1 Y, 2 Z, 3 magx, 4 magy, 5 magz, 6 mag_xyz, 7 ori
    """
    pos_last = pos_mag[0,1:4]
    mag_last = pos_mag[0,6:9]
    ori_last = pos_mag[0,4]
    pos_grid = []
    for i in range(1, pos_mag.shape[0]):
        pos_cur = pos_mag[i,1:4]
        mag_cur = pos_mag[i,6:9]
        ori_cur = pos_mag[i,4]
        pos_delta = np.linalg.norm(pos_cur - pos_last)
        if pos_delta == 0:
            continue
        pos_vec = (pos_cur - pos_last)/pos_delta
        mag_delta = mag_cur - mag_last
        ori_delta = ori_cur - ori_last
        mag_vec = mag_delta/pos_delta
        ori_vec = ori_delta/pos_delta
        # print(i)
        while(pos_delta > 1):
            pos_delta -= 1
            pos_last +=  pos_vec
            mag_last += mag_vec
            ori_last += ori_vec
            pos_grid.append(np.hstack((pos_last,ori_last,mag_last,np.linalg.norm(mag_last) )))
    return np.array(pos_grid)
        
def grid_pos(pos):
    """将轨迹格点化，格子大小为1 m
    
    Args:
        pos_mag: #0 time, 1 X, 2 Y, 3 Z, 4 ori
        
        
    Returns:
        格点化后的格子数据: #0 time, 1 X, 2 Y, 3 Z, 4 ori
    """

    len_grid = 0.1  #格点的长度

    time_last = pos[0,0]
    pos_last = pos[0,1:4]
    # mag_last = pos_mag[0,6:9]
    ori_last = pos[0,4]

    pos_grid = []
    pos_grid.append(np.hstack((time_last, pos_last, ori_last)))
    
    for i in range(1, pos.shape[0]):
        pos_cur = pos[i,1:4]
        # mag_cur = pos_mag[i,6:9]
        ori_cur = pos[i,4]
        time_cur = pos[i,0]
        pos_delta = np.linalg.norm(pos_cur - pos_last)
        if pos_delta == 0:
            continue
        # print('pos_delta:',pos_delta)
        # print('pos_cur:',pos_cur)
        # print('pos_last:',pos_last)
        pos_vec = (pos_cur - pos_last)*len_grid/pos_delta
        # mag_delta = mag_cur - mag_last
        ori_delta = (ori_cur - ori_last) % 360
        if ori_delta > 180:
            ori_delta = ori_delta - 360
        time_delta = time_cur - time_last
        # mag_vec = mag_delta/pos_delta
        ori_vec = ori_delta*len_grid/pos_delta
        time_vec = time_delta*len_grid/pos_delta
        # print(i)
        while(pos_delta > len_grid):
            pos_delta -= len_grid
            pos_last =  pos_last + pos_vec
            # mag_last += mag_vec
            ori_last = (ori_last + ori_vec) % 360
            time_last += time_vec
            pos_grid.append(np.hstack((time_last, pos_last, ori_last)))
            # print('Time:',time_last)
            # print('pos_delta:',pos_delta, (np.linalg.norm(pos_cur - pos_last)))
    return np.array(pos_grid)
        
def corner_detected(pos_grid):
    '''
    file    :curve_line_segmentation_f.py
    descri  :检测车辆转弯
    time    :2021/07/19 11:27:08
    author :binyao
    version :1.0
    input   :格点化的位置、地磁和朝向的数据 
             # 0 X, 1 Y, 2 Z, 3 ori, 4 magx, 5 magy, 6 magz, 7 mag_xyz
    output  :转弯起始点和结束点在格点数据中的位置
             
    '''
    ang_corner = 70
    max_len = 210
    half_max_len = max_len//2
    ori = pos_grid[:,3]
    begin = 0
    end = begin + max_len
    length = pos_grid.shape[0]

    res = []
    # cor_central = []
    while length > end:
        ang_delta = abs(ori[begin] - ori[end])
        if ang_delta > 360 - ang_corner:
            ang_delta = 360 - ang_delta
        if ang_delta > ang_corner:
            tmp_p = abs(np.diff(ori[begin:end+1]))
            max_index = np.argsort(tmp_p)[-1] + begin
            # cor_central.append(max_index)
            res.append([max_index-half_max_len, max_index+half_max_len])
            # begin = np.searchsorted(ori[begin:end], ori[begin], side='right') - 1 + begin
            # res.append([begin, end])
            begin = max_index + half_max_len
            end = begin
        else:
            if end - begin >= max_len:
                begin += 1
                end += 1
            else:
                end += 1
    return np.array(res)

def pos_update(corner,pos_grid, vis_cor = True,vis_line = True):
    pass
def angleDiff180(a, b):

    p_thresh = 15 #平行直线间的航向差

    ang_delta = (a - b)%360
    if ang_delta > 180:
        ang_delta = 360 - ang_delta
    return ang_delta < p_thresh
def recAngle(a,b):
    if angleDiff180(a, b):
        pass
    elif angleDiff180((a+90)%360, b):
        a = (a+90)%360
    elif angleDiff180((a+180)%360, b):
        a = (a+180)%360
    elif angleDiff180((a+270)%360, b):
        a = (a+270)%360
    return a
def linePosUpdata():
    pass
def lineFilter(pos_grid, pos_grid_copy):
    '''
    file    :curve_line_segmentation_f.py
    descri  :直线过滤器，应用场景局限在矩形状的环境中
    time    :2021/11/08 20:47:47
    author :binyao
    version :1.0
    input   :
    output  :
    '''
    # import math
    # import matplotlib.pyplot as plt
    # fig = plt.figure(figsize=(16, 16))
    # plt.ion()
    # image_list = []
    # # for i in range(10, corner.shape[0] - 1):
    # for i in range(10,len(corner)-1):
       
    #         start, stop = corner[i,0], corner[i,1]
    #         plt.plot(pos_grid[start:stop+1,0],pos_grid[start:stop+1,1], marker=',')
    #         plt.show()
    #         plt.pause(1)
        
    #         start, stop = corner[i,1], corner[i+1,0]
    #         plt.plot(pos_grid[start:stop+1,0],pos_grid[start:stop+1,1], marker=',')
    #         plt.show()
    #         plt.pause(0.5)
    #         plt.savefig('temp.png')
    ori = pos_grid_copy[:,3]
    ang_corner = 10
    indoor = 3550 #第一圈室内
    
    window_slide = 100
    for i in range(indoor,len(pos_grid) - window_slide, int(window_slide/2)):#找第一条直线
        ang_delta = (ori[i + window_slide] - ori[i])% 360
        if ang_delta > 180:
            ang_delta = 360 - ang_delta
        if ang_delta < ang_corner:
            theta_last = np.average(ori[i:i + window_slide])
            break
    for i in range(i, len(pos_grid) - int(window_slide/2) - 2500, int(window_slide/2)):
        ang_delta = (ori[i + window_slide] - ori[i])% 360
        if ang_delta > 180:
            ang_delta = 360 - ang_delta
        if ang_delta < ang_corner:
            # if max(np.diff(ori[i:i + window_slide])) < 2: #相邻位姿间的航向变化不超过5度
                theta_now = np.average(ori[i:i + window_slide])
                theta_update = recAngle(theta_last, theta_now)
                pos_grid[i,3] = theta_update
                #inv(v2t(a))*v2t(b) 计算由a到b的转移矩阵T
                for j in range(i,i + window_slide):
                    c = [pos_grid[j,0], pos_grid[j,1], math.radians(wrapTo180(pos_grid[j,3]))]
                    a = [pos_grid_copy[j,0], pos_grid_copy[j,1], math.radians(wrapTo180(pos_grid_copy[j,3]))]
                    j += 1
                    b = [pos_grid_copy[j,0], pos_grid_copy[j,1], math.radians(wrapTo180(pos_grid_copy[j,3]))]
                     
                    T = np.dot(np.linalg.inv(v2t(a)),v2t(b))
                    res = t2v(np.dot(v2t(c),T))
                    pos_grid[j,:2] = res[:2]
                    pos_grid[j,3] =  math.degrees(res[2])%360
                theta_last = theta_update
        else:
            for j in range(i,i + window_slide):
                    c = [pos_grid[j,0], pos_grid[j,1], math.radians(wrapTo180(pos_grid[j,3]))]
                    a = [pos_grid_copy[j,0], pos_grid_copy[j,1], math.radians(wrapTo180(pos_grid_copy[j,3]))]
                    j += 1
                    b = [pos_grid_copy[j,0], pos_grid_copy[j,1], math.radians(wrapTo180(pos_grid_copy[j,3]))]
                     
                    T = np.dot(np.linalg.inv(v2t(a)),v2t(b))
                    res = t2v(np.dot(v2t(c),T))
                    pos_grid[j,:2] = res[:2]
                    pos_grid[j,3] = math.degrees(res[2])%360
                # theta_last = theta_update
   ##室外部分不做直线约束
    for j in range(i + window_slide, len(pos_grid)-1):
            c = [pos_grid[j,0], pos_grid[j,1], math.radians(wrapTo180(pos_grid[j,3]))]
            a = [pos_grid_copy[j,0], pos_grid_copy[j,1], math.radians(wrapTo180(pos_grid_copy[j,3]))]
            j += 1
            b = [pos_grid_copy[j,0], pos_grid_copy[j,1], math.radians(wrapTo180(pos_grid_copy[j,3]))]
                
            T = np.dot(np.linalg.inv(v2t(a)),v2t(b))
            res = t2v(np.dot(v2t(c),T))
            pos_grid[j,:2] = res[:2]
            pos_grid[j,3] = math.degrees(res[2])%360


